#include <stdio.h>
#include <math.h>
#include "../utils/BasicType.h"

#define GRAVITY 9.81
#define ANGULAR_VELOCITY_THRESHOLD 0.01
#define ACCELERATION_THRESHOLD 0.1              // slight iir filtering to clear spikes.
#define DELTA_ANGULAR_VELOCITY_THRESHOLD 0.01
#define DELTA_ACCELERATION_THRESHOLD 0.05
#define GRAV_AUTOSCALE_THRESHOLD 1.0
#define ACC_FILT_TIMECONSTANT 0.1
#define GRAV_AUTOSCALE_ALPHA 0.005
#define STEADY_WAIT_THRESHOLD 0.2


typedef struct ComplementaryFilterV2{
    double gain_acc;               // 加速度初始增益
    double prev_gain_acc;          // 上一增益值
    double gain_mag;               // 磁力计初始增益
    double bias_alpha;             // 零偏估计参数
    int do_bias_estimation;        // 是否零偏估计
    int do_adaptive_gain;          // 是否自适应计算增益
    int do_gravity_autoscale;      // 是否自适应调整重力加速度
    double gravity;                // 重力加速度
    int initialized;               // 是否初始化
    int steady_state;              // 是否处于稳定状态
    int partial_steady_state;      // 是否处于半稳定状态
    Quaternion *q;                 // 朝向四元数 q0是实部
    Vector3* a_filt;               // 滤波后的加速度
    Vector3* a_prev;               // 前一个加速度
    Vector3* w_prev;                // 前一个角速度
    Vector3* w_bias;                // 角速度零偏
    double time;                   // 当前时间
    double time_steady;            // 稳定状态时长
    double initial_settle_time;    // 加速度增益的初始时间
    double angle_velocity;          //角速度
} ComplementaryFilterV2;

void initialize(ComplementaryFilterV2 *filter);

void initComplementaryFilter(ComplementaryFilterV2 * filter);

int set_gain_acc(ComplementaryFilterV2 *filter, double gain);

int set_gain_mag(ComplementaryFilterV2 *filter, double gain);

int set_bias_alpha(ComplementaryFilterV2 *filter, double bias_alpha);

void set_initial_settle_time(ComplementaryFilterV2 *filter, double settle_time);

int get_steady_state(const ComplementaryFilterV2 *filter);

Quaternion get_orientation(const ComplementaryFilterV2 *filter);

void set_orientation(const ComplementaryFilterV2 *filter, double q0, double q1, double q2, double q3);

void normalize_vector(Vector3 *v);

void normalize_quaternion(Quaternion *q);

Quaternion get_prediction(Quaternion *q, Vector3 *w_bias, double wx, double wy, double wz, double dt);

Vector3 rotate_vector_by_quaternion(Vector3 *acc, Quaternion *q);

void scale_quaternion(double gain, Quaternion *q);

void quaternion_multiplication(Quaternion *q, Quaternion *p, Quaternion *d);

void filter_acc(Vector3 *a_filt, double ax, double ay, double az, double dt);

void update_biases(Vector3 *w_bias, double bias_alpha, double time_steady, double wx, double wy, double wz);

void autoscale_gravity(int partial_steady_state, Vector3 *a_filt, double *gravity);

int check_state(ComplementaryFilterV2 *filter, double ax, double ay, double az, double wx, double wy, double wz);

double get_adaptive_gain(ComplementaryFilterV2 *filter, double ax, double ay, double az, double dt);

void set_vector3(double ax, double ay, double az, Vector3 * vector3);

void update(ComplementaryFilterV2 *filter, double ax, double ay, double az, double wx, double wy, double wz, double dt);

